

#include <TinyGPS.h>
#include <SPI.h>
#include <inttypes.h>
#include <Arduino.h>

#include "global.h"
#include "LOG.h"
#include "WiFi.h"
#include "IMU.h"
#include "ExtPROM.h"
#include "WARDEN.h"

//TESTING CONFIGS
	//#define tst_WRITE //fill log with example data
		//#define tst_WR_VERBOSE
	//#define tst_READ  //print entire log		
		//#define tst_RD_VERBOSE

	#define def_LED_BYTE
//	#define def_PRINTMODE
	#define def_LOGMODE
	#define NUM_STORED 3
	#define GPS_CONFINDENCE_WEIGHT 0.1	// The higher this value, the more weight the GPS confidence has
	#define SYNC_TIME 5000					// The length of time in ms until the GPS and IMU sync coordinates
	#define IMU_CONFIDENCE_CUTOFF 0.5		// The IMU confidence level (before adjustment) at the time of sync

	#ifdef def_LED_BYTE
		int toggles[8]; 
		void toggle(int);
		void byteLite(uint8_t);
	#endif
	

//
//Functional variables & defines
//
	//Objects
	Log SpiLOG;
	Warden warden_master;
	TinyGPS GPS_handler;
	IMU IMU_handler;
	WiFi wifi_handler;

	//Global variables
	enum {serialMode_self, serialMode_wifi, serialMode_GPS, serialMode_IMU};  //look who's talking
	int glblSerialMode;

	int serialBaud;

	bool flgNewGPS;
	int numGPS;
	unsigned long timeTweenGPS;
	unsigned long timeRxGPS;

	bool flgNewIMU;
	int numIMU;
	unsigned long timeTweenIMU;
	unsigned long timeRxIMU;

	int addressWriting=0;
	bool flgBuffFull=false;
	bool flgBuffChoice=true;
	LogBuff IMUraw0;
	LogBuff IMUraw1;
	
	//runtime debug data
	int numLoops;
	int numReadHolds;
	int numRdyWrHolds;
	int numWrenWrHolds;

	//RX call-back function
	int busStation();

	//Coordinate and Vector Arrays
	Coordinate coords[NUM_STORED];
	Vector vecs[NUM_STORED];

	//Time for GPS/IMU integration
	unsigned long lastSync = 0;


void setup()
{
	serialBaud = 115200;
	#ifdef def_PRINTMODE
		Serial.begin(serialBaud);
	#endif

	#ifndef def_PRINTMODE
		Serial.beginWithInterrupt(serialBaud, busStation);
	#endif

	//extEEPROM chip select
	pinMode(10, OUTPUT); 
		//and the rest of SPI
	pinMode(11, OUTPUT);
	pinMode(12, INPUT);
	pinMode(13, OUTPUT);

	//rs485 chips
	pinMode(def_selfChip,OUTPUT); //arduino (self)
	pinMode(def_wifiChip,OUTPUT); //wifi
	pinMode(def_GPSChip,OUTPUT); //GPS
	pinMode(def_IMUChip,OUTPUT); //IMU

	serialMode(serialMode_IMU);

	#ifdef def_LED_BYTE
	//pinMode(2, OUTPUT);
	//pinMode(3, OUTPUT);
	//pinMode(4, OUTPUT);
	//pinMode(5, OUTPUT);
	pinMode(6, OUTPUT);		// no more byteLite
	pinMode(7, OUTPUT);		// no more byteLite
	pinMode(8, OUTPUT);		// no more byteLite
	pinMode(9, OUTPUT);		// used to play tones on the speaker
	#endif

	SpiPROM.init(10);

	flgNewGPS=false;
	numGPS=1;
	timeTweenGPS=0;
	timeRxGPS=0;


	flgNewIMU=false;
	numIMU=0;
	timeTweenIMU=0;
	timeRxIMU=0;

	flgBuffFull = false;

	numLoops=0;
	numReadHolds=0;
	numRdyWrHolds=0;
	numWrenWrHolds=0;
	//byteLite(numGPS);
}


void loop()
{
	if(flgBuffFull){
		if(flgBuffChoice)
			SpiPROM.writeLogBuff(addressWriting, IMUraw1);
		else
			SpiPROM.writeLogBuff(addressWriting, IMUraw0);

		addressWriting+=128;
	}

	if(flgNewGPS)
	{
		long lat, lon;
		unsigned long fixage;
		timeRxGPS = millis();
		GPS_handler.get_position(&lat, &lon, &fixage);
		Coordinate currentC;
		currentC.lat = lat;
		currentC.lon = lon;
		currentC.confidence = GPS_handler.hdop();
		addCoord(currentC);		//Adds current coordinate to array and shifts previous entries
		//SpiLOG.addGPS(lat, lon, GPS_handler.hdop());

		//if(numGPS<2){  //use this to find GPS interval
		//	if(numGPS==0)
		//		timeTweenGPS = timeRxGPS;
		//	if(numGPS==1)
		//		timeTweenGPS = timeRxGPS - timeTweenGPS;
		//		IMU_handler.init(timeTweenGPS);
		//	//then use 485 chips to switch over to the IMU and take a reading
		//}

		numGPS++;
		//byteLite(numGPS);

		flgNewGPS=false;
		//serialMode(serialMode_IMU);
	}

	if(flgNewIMU)
	{
		timeRxIMU = millis();

		SpiLOG.addIMU(3,2,1,-2,33,90,359,59);

		numIMU++;

		serialMode(serialMode_GPS);

		Vector currentV;
		//currentV.ang = ?		THESE NEED TO BE UPDATED
		//currentV.mag = ?		THESE NEED TO BE UPDATED
		addVect(currentV);
	}

	if((millis()-lastSync) > 100)	//Time to sync GPS/IMU
	{
		lastSync = millis();
		Coordinate IMU = combine(coords[1], vecs[0]);
		Coordinate c = merge(coords[0], IMU, 4999);		// We can experiment with the 4999 value. It is IMU confidence (0-4999, with 0 being most confident in IMU)
		if(warden_master.onSameSide(c))
		{
			if(warden_master.shortestDistance(c) < DANGER_ZONE_DIST)
			{
				// YELLOW (play buffer tone)
				tone(9, 1760, 100);
			}
			else
			{
				// GREEN (do nothing)
			}
		}
		else
		{
			// RED (play outside tone)
			tone(9, 3520, 100);
		}
	}

#ifdef def_PRINTMODE
	//fault information
		Serial.println(numRdyWrHolds);
		Serial.println(numWrenWrHolds);
		Serial.println(numReadHolds);
		Serial.println();
#endif
}

Coordinate combine (Coordinate c, Vector v)
{
	float x, y;
	x = v.mag*cos(v.ang);
	y = v.mag*sin(v.ang);
	Coordinate final;
	final.lat = c.lat + x;		// SOMEBODY PLEASE CHECK WHETHER X OR Y SHOULD BE ADDED TO LATITUDE, IM WAYYYY TOOO TIREDDDD
	final.lon = c.lon + y;
	return final;
}

Coordinate merge (Coordinate GPS, Coordinate IMU, float time)
{
	float GPSconfidence = GPS.confidence;
	float coef = (1-IMU_CONFIDENCE_CUTOFF) / (SYNC_TIME*SYNC_TIME);
	float IMUconfidence = 1 - (coef * (time*time));
	GPSconfidence *= GPS_CONFINDENCE_WEIGHT;
	float finalConfidence;
	finalConfidence = IMUconfidence - (GPSconfidence*IMUconfidence);

	return pointOnLine(GPS, IMU, finalConfidence);
}

Coordinate pointOnLine (Coordinate c1, Coordinate c2, float percent)
{
	Coordinate vector;
	Coordinate final;
	vector.lat = percent*(c2.lat-c1.lat);
	vector.lon = percent*(c2.lon-c1.lon);
	final.lat = c1.lat + vector.lat;
	final.lon = c1.lon + vector.lon;
	return final;
}

void addCoord (Coordinate c)
{
	for(int i=NUM_STORED-1; i>0; i--)
	{
		coords[i] = coords[i-1];
	}
	coords[0] = c;
}

void addVect (Vector v)
{
	for(int i=NUM_STORED-1; i>0; i--)
	{
		vecs[i] = vecs[i-1];
	}
	vecs[0] = v;
}


//
//uart & rs485 accessories
//
void serialMode(int mode){  //ensures bus station receives collision free data, could be set to change channel at certain times

	switch(mode) {
		case serialMode_self:
			digitalWrite(def_selfChip, HIGH);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_wifi:
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, HIGH);
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_GPS:
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, HIGH);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_IMU:
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, HIGH);
			break;
		default: break;
	}
	
	glblSerialMode = mode;
}

int busStation(){     //called with each new byte in
	int c = Serial.read();
	static int byteCount=0;
	if(glblSerialMode==serialMode_self){
		//if (SELFhandler.encode(c))
		//{
		//	flgNewSelf = true;
		//}
	}
	else if(glblSerialMode==serialMode_wifi){
		//if (WIFIhandler.encode(c))
		//{
		//	flgNewWIFI = true;
		//}
	}
	else if(glblSerialMode==serialMode_GPS){
		if (GPS_handler.encode(c))
		{
			flgNewGPS=true;
		}
	}
	else if(glblSerialMode==serialMode_IMU){
		if(flgBuffChoice)
			IMUraw0.data[byteCount]=Serial.read();
		else
			IMUraw1.data[byteCount]=Serial.read();
		byteCount++;
		if(byteCount==128){
			flgBuffChoice=!flgBuffChoice;
			flgBuffFull=true;
			byteCount=0;
		}

		//if (IMU_handler.encode(c))
		//{
		//	flgNewIMU = true;
		//}
	}

	return 1;
}

#ifdef def_LED_BYTE
	void toggle(int bit){
		if(!toggles[bit]){
			digitalWrite((BYTE_LED_LSB+bit), HIGH);
			toggles[bit]=1;
		}

		else{
			digitalWrite((BYTE_LED_LSB+bit), LOW);
			toggles[bit]=0;
		}
	}

	void byteLite(uint8_t byteQ){
		for(int i=0; i<4; i++){
			if(((byteQ>>i) & 0x01)==0x01)
				digitalWrite(BYTE_LED_LSB+i, HIGH);
			else digitalWrite(BYTE_LED_LSB+i, LOW);
		}
	}
#endif